package robocup.component.geometry;

import java.awt.geom.Point2D;

/**
 * Simple Vector. Points at x,y.
 * 
 * The angle to the y-axis is the vector's angle due to the dainamite coordinate
 * system (see Manual).
 */
public class Vektor {

  /**
   * the x-coordinate
   */
  public double              x;

  /**
   * the y-coordinate
   */
  public double              y;

  /** ************************** DUMMIES ************************* */

  /** ************************** STATICS ************************* */

  /**
   * <code>POLAR</code> polar coordinates flag
   */
  public final static int    POLAR  = 1;

  /**
   * <code>XY</code> cartesian coordinates flag
   */
  public final static int    XY     = 2;

  /**
   * the coordinate center
   */
  public static final Vektor CENTER = new Vektor();

  /** ************************** CONSTRUCTORS ******************** */

  /**
   * new Zero-vector
   */
  public Vektor() {

    this.x = 0;
    this.y = 0;
  }

  /**
   * Vektor from polar coordinates. <code>iAngle</code> is not the usual
   * vector angle, but (90degree - vector angle) - the angle (in degrees) to the
   * Y-axis.
   * 
   * @param iLength -
   *          length of the vector
   * @param iAngle -
   *          angle (in degrees) to Y-axis
   */
  public Vektor(
      final double iLength,
      final double iAngle) {

    this.y = iLength * Math.cos(Math.toRadians(iAngle));
    this.x = iLength * Math.sin(Math.toRadians(iAngle));
  }

  /**
   * Vektor created from polar or cartesian coordinates
   * 
   * @param type -
   *          polar/cartesian
   * @param XorLength -
   *          X-coordinate or length of the vector
   * @param YorAngle -
   *          Y-coordinate or angle (in degrees) of the vector
   */
  public Vektor(
      final int type,
      final double XorLength,
      final double YorAngle) {

    if (type == Vektor.XY) { // XY-koordinaten
      this.x = XorLength;
      this.y = YorAngle;
    }
    else {// Polarkoordinaten
      this.y = XorLength * Math.cos(Math.toRadians(YorAngle));
      this.x = XorLength * Math.sin(Math.toRadians(YorAngle));
    }
  }

  /**
   * <code>Vektor</code> constructor creates a Vektor from a 2D point.
   * 
   * @param point
   *          Point2D representation of the vector
   */
  public Vektor(
      final Point2D point) {

    this(Vektor.XY, point.getX(), point.getY());
  }

  /** ************************** STATICS ************************* */

  /**
   * This method normalizes an angle.
   * 
   * @param angle
   *          The angle to normalize.
   * @return An angle with -180 <= angle <= 180.
   */
  public final static double normalize(double angle) {

    while (angle < -180) {
      angle += 360;
    }
    while (angle > 180) {
      angle -= 360;
    }
    return angle;
  }

  /**
   * Calculates if an angle lies between start and end. Between start and end
   * are all angles, that can be reached by increasing the start angle up to
   * end.
   * 
   * @param start
   *          the smaller angle
   * @param between
   *          the test angle
   * @param end
   *          the bigger angle
   * @return true if the test angle is between the others
   */
  public static final boolean isAngleBetween(double start,
      double between,
      double end) {

    start = Vektor.normalize(start);
    between = Vektor.normalize(between);
    end = Vektor.normalize(end);
    if (start <= end && start <= between && between <= end) {
      return true;
    }
    if (start > end && (start <= between || between <= end)) {
      return true;
    }
    return false;
  }

  /**
   * creates an Vektor object. (same as "new Vektor()").
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * @return a new Vektor
   */
  public final static Vektor getDummyInstance() {

    return new Vektor();
  }

  /**
   * creates an Array of Vektors
   * 
   * <br>
   * NEW OBJECTS: number
   * 
   * @param number
   *          of Vektors
   * @return an Vektor[] of length number
   */
  public static final Vektor[] getDummyInstances(final int number) {

    final Vektor[] vektors = new Vektor[number];
    for (int i = 0; i < vektors.length; i++) {
      vektors[i] = new Vektor();
    }
    return vektors;
  }

  /** ************************** METHODS ************************* */

  /**
   * sets the vektor's data avoiding the creation of a new instance.
   * 
   * @param dist
   * @param angle
   */
  public final void setVektor(final double dist,
      final double angle) {

    this.y = dist * Math.cos(Math.toRadians(angle));
    this.x = dist * Math.sin(Math.toRadians(angle));
  }

  /**
   * Clones a Vektor without creating a new one.
   * 
   * @param v
   *          the Vektor to copy
   * @return this vektor
   */
  public final Vektor copy(final Vektor v) {

    this.x = v.x;
    this.y = v.y;

    return this;
  }

  /**
   * sets the Vektor to (0,0)
   * 
   */
  public void reset() {

    this.x = 0;
    this.y = 0;
  }

  /**
   * pointAt is a setter for the coordinates <code>x</code> & <code>y</code>
   * 
   * @param x_value
   * @param y_value
   */
  public final void pointAt(final double x_value,
      final double y_value) {

    this.x = x_value;
    this.y = y_value;
  }

  /**
   * set the Vektor by polar data.
   * 
   * @param length
   * @param angle
   */
  public final void pointAtPolar(final double length,
      final double angle) {

    this.y = length * Math.cos(Math.toRadians(angle));
    this.x = length * Math.sin(Math.toRadians(angle));
  }

  /**
   * mult multiplies the coordinates of the Vektor with factor <code>a</code>
   * 
   * @param a -
   *          factor
   * @return this Vektor
   */
  public final Vektor multThis(final double a) {

    this.x *= a;
    this.y *= a;
    return this;
  }

  /**
   * as mult but returns the result without changing the instance itself.
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * @param a -
   *          factor
   * @return new Vektor with same dir but length = old_length * a;
   */
  public final Vektor mult(final double a) {

    return new Vektor(Vektor.XY, this.x * a, this.y * a);
  }

  /**
   * <code>divThis</code> divides the coordinates of this Vektor
   * 
   * @param divider
   *          divide factor
   */
  public final void divThis(final double divider) {

    this.x /= divider;
    this.y /= divider;
  }

  /**
   * <code>div</code>
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * @param divider
   * @return Vektor
   */
  public final Vektor div(final double divider) {

    return new Vektor(Vektor.XY, this.x / divider, this.y / divider);
  }

  /**
   * sub returns a new Vektor equal to <code>this</code> - <code>v</code>
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * @param v -
   *          Vektor to substract
   * @return Vektor - <code>this</code> - <code>v</code>
   */
  public final Vektor sub(final Vektor v) {

    return new Vektor(Vektor.XY, this.x - v.x, this.y - v.y);
  }

  /**
   * sub returns <code>this</code> after <code>v</code> was sustracted from
   * <code>this</code>. No new vektor instance is created.
   * 
   * @param v -
   *          Vektor to substract
   * @return Vektor - <code>this</code> - <code>v</code>
   */
  public final Vektor subFromThis(final Vektor v) {

    this.x -= v.x;
    this.y -= v.y;
    return this;
  }

  /**
   * add returns a new Vektor equal to <code>this</code> + <code>v</code>
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * @param v -
   *          Vektor to add
   * @return Vektor - <code>this</code> + <code>v</code>
   */
  public final Vektor add(final Vektor v) {

    return new Vektor(Vektor.XY, this.x + v.x, this.y + v.y);
  }

  /**
   * addToThis returns <code>this</code> Vektor after <code>v</code> was
   * added to this. No new vektor instance is created.
   * 
   * @param v -
   *          Vektor to add
   * @return Vektor - <code>this</code> + <code>v</code>
   */
  public final Vektor addToThis(final Vektor v) {

    this.x += v.x;
    this.y += v.y;
    return this;
  }

  /**
   * returns the distance to a given vektor
   * 
   * @param v
   * @return distance to v
   */
  public final double getDistance(final Vektor v) {

    return Math.sqrt((this.x - v.x)
        * (this.x - v.x)
        + (this.y - v.y)
        * (this.y - v.y));
  }

  /**
   * cross returns the cross product with an vektor
   * 
   * @param v
   *          2. Vektor
   * @return cross product
   */
  public final double cross(final Vektor v) {

    return (this.x * v.y - this.y * v.x);
  }

  /**
   * scalar of 2 Vektors
   * 
   * @param v
   * @return the scalar
   */
  public final double scalar(final Vektor v) {

    return this.x * v.x + this.y * v.y;
  }

  /**
   * getLength returns the length of the vector
   * 
   * @return length
   */
  public final double getLength() {

    return (Math.sqrt(this.x * this.x + this.y * this.y));
  }

  /**
   * getAngle returns the angle (in degrees) of the vector to the Y-Axis
   * Absolute: Angle between the vector and the goal-goal-line
   * 
   * @return angle to Y-axis in degree
   */
  public final double getAngle() {

    if (this.x == 0.0 && this.y == 0) {
      return 0.0;
    }
    return Math.atan2(this.x, this.y) * (180.0 / Math.PI);
  }

  /**
   * Returns a Vektor from this Vektor to the global Vektor <code>v</code>.
   * Note that this method is equal to <code>v</code>.sub(<code>this</code>).
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * @param target -
   *          global Vektor
   * @return from this Vektor to the global Vektor <code>v</code>
   */
  public final Vektor getVektorTo(final Vektor target) {

    return target.sub(this);
  }

  /**
   * Returns a Vektor from this Vektor to the global Vektor <code>v</code>.
   * Note that this method is equal to <code>v</code>.sub(<code>this</code>).
   * 
   * @param target -
   *          global Vektor
   * @param result -
   *          here the result is saved (overwrites old contents)
   */
  public final void getVektorTo(final Vektor target,
      final Vektor result) {

    result.copy(target);
    result.subFromThis(this);
  }

  /**
   * Returns the global angle to the specified Vektor <code>v</code> relative
   * to the position denoted by this Vektor.
   * 
   * @param v -
   *          global Vektor
   * @return double - global angle from this Vektor to the global position
   *         specified by the Vektor <code>v</code>.
   */
  public final double getAngleTo(final Vektor v) {

    if (this.x == v.x && this.y == v.y) {
      return 0.0;
    }
    return Math.atan2(v.x - this.x, v.y - this.y) * (180.0 / Math.PI);
  }

  /**
   * Returns the global angle between the angle of this Vektor and the angle of
   * the specified Vektor <code>v</code>.
   * 
   * @param v -
   *          Vektor
   * @return double - global angle between both vektors
   */
  public final double getAngleBetween(final Vektor v) {

    return Math.abs(Vektor.normalize(this.getAngle() - v.getAngle()));
  }

  /**
   * returns the angle between to positions seen from the Vektor.
   * 
   * @param v1 -
   *          left position
   * @param v2 -
   *          right position
   * @return angle between v1 and v2 seen from this vektor
   */
  public final double getAngleBetween(final Vektor v1,
      final Vektor v2) {

    return Vektor.normalize(this.getAngleTo(v2) - this.getAngleTo(v1));
  }

  /**
   * This method returns a Vektor that is interjacent to the Vektor and the
   * given Vektor <code>v</code>.
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * @param v
   *          second Vektor
   * @return the Vektor between v and this
   */
  public final Vektor getVektorBetween(final Vektor v) {

    return new Vektor(Vektor.XY, (this.x + v.x) / 2, (this.y + v.y) / 2);
  }

  /**
   * This method returns a Vektor that is interjacent to the Vektor and the
   * given Vektor <code>v</code>.
   * 
   * @param v
   *          second Vektor
   * @param result
   *          here the result is saved (overwrites old contents)
   */
  public final void getVektorBetween(final Vektor v,
      final Vektor result) {

    result.x = (this.x + v.x) / 2;
    result.y = (this.y + v.y) / 2;
  }

  /**
   * setLength sets the length of the vector to <code>length</code>
   * 
   * @param length -
   *          new length
   * @return a Vektor of the given length
   */
  public final Vektor setLength(final double length) {

    final double thisLength = this.getLength();
    if (thisLength == 0) {
      if (length == 0) {
        return this;
      }
      this.x = 0;
      this.y = length;
      return this;
    }

    final double rel = length / thisLength;
    this.x *= rel;
    this.y *= rel;
    return this;
  }

  /**
   * Rotates the vector at the angle of alpha
   * 
   * @param alpha -
   *          angle of the rotation
   */
  public final void rotate(final double alpha) {

    final double length = this.getLength();
    final double angle = Math.toRadians(this.getAngle() + alpha);
    this.y = length * Math.cos(angle);
    this.x = length * Math.sin(angle);

    // Vektor temp = new Vektor(this.getLength(), this.getAngle() + alpha);
    // this.x = temp.x;
    // this.y = temp.y;
  } // end of method rotate(int alpha)

  /**
   * Clones the Vektor.
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * @return Clone
   */
  public final Vektor cloned() {

    return new Vektor(Vektor.XY, this.x, this.y);
  }

  /**
   * sets the Vektor to "empty"
   * 
   */
  public final void setEmpty() {

    this.x = java.lang.Double.MAX_VALUE;
  }

  /**
   * tests if Vektor is "empty"
   * 
   * @return true, if no value is saved in the Vektor
   */
  public final boolean isEmpty() {

    return this.x == java.lang.Double.MAX_VALUE;
  }

  /**
   * 
   * @return a String representing this Vektor
   * @see robocup.component.infotypes.Info#toString()
   */
  @Override
  public final String toString() {

    return ("("
        + Math.rint(this.x * 100)
        / 100
        + ", "
        + Math.rint(this.y * 100)
        / 100 + ")");
  }

  /**
   * toSting() with only n digits for doubles
   * 
   * @param n -
   *          Number of digits
   * @return a String using precision n and representing this Vektor
   */
  public final String toString(final int n) {

    final double a = Math.pow(10.0, n);
    return ("(" + Math.rint(this.x * a) / a + ", " + Math.rint(this.y * a) / a + ")");
  }

  /**
   * <code>getPoint</code> returns a Point2D representation of the Vektor
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * @return Point2D
   */
  public final Point2D getPoint() {

    return new Point2D.Double(this.x, this.y);
  }

  /**
   * checks if the given vektor <code>v</code> lies in a environment with size
   * of <code>variation</code>
   * 
   * @param v
   * @param variation
   * @return true if the Vektor liesin the environment
   */
  public final boolean compareWithVariation(final Vektor v,
      final double variation) {

    return (this.getDistance(v) <= variation);
  }

  /**
   * @return Returns the x.
   */
  public final double getX() {

    return this.x;
  }

  /**
   * @param x
   *          The x to set.
   */
  public final void setX(final double x) {

    this.x = x;
  }

  /**
   * @return Returns the y.
   */
  public final double getY() {

    return this.y;
  }

  /**
   * @param y
   *          The y to set.
   */
  public final void setY(final double y) {

    this.y = y;
  }

  /**
   * Calculates if this Vektor and p mark the same position
   * 
   * @param p
   * @return true if this Vektor and p mark the ame position, false otherwise.
   */
  public final boolean equals(Vektor p) {

    if (this.x != p.x) {
      return false;
    }
    if (this.y != p.y) {
      return false;
    }
    return true;
  }

  /**
   * Calculates the distance from ang1 to ang2 in increasing direction.
   * For example, the distance from 170 to 140 is NOT 30 but 330 since we increase from 170 until we reach 140 again.
   * 
   * @param ang1 the first(smaller) angle
   * @param ang2 the second(bigger) angle
   * 
   * @return the distance between the angles
   */
  public static final double getAngleDistance(final double ang1,
      final double ang2) {
    if (ang2 >= ang1) {
      return ang2 - ang1;
    }
    return 360 - ang1 + ang2;
  }
} // end of class Vektor
